#! /usr/bin/python
# Copyright (c) 2015, Rethink Robotics, Inc.

# Using this CvBridge Tutorial for converting
# ROS images to OpenCV2 images
# http://wiki.ros.org/cv_bridge/Tutorials/ConvertingBetweenROSImagesAndOpenCVImagesPython

# Using this OpenCV2 tutorial for saving Images:
# http://opencv-python-tutroals.readthedocs.org/en/latest/py_tutorials/py_gui/py_image_display/py_image_display.html

# rospy for the subscriber
import rospy
# ROS Image message
from sensor_msgs.msg import Image
# ROS Image message -> OpenCV2 image converter
from cv_bridge import CvBridge, CvBridgeError
# OpenCV2 for saving an image
import cv2
from std_msgs.msg import String
from time import sleep

count = 3
# Instantiate CvBridge
bridge = CvBridge()
pub = rospy.Publisher("/taking_photo",String,queue_size=10)

def image_callback(msg):
    global count
    rootpath = "/home/agilex/Pictures/ros_nav/"
    if count <= 0:
        return
    print("Received an image!")
    try:
        # Convert your ROS Image message to OpenCV2
        cv2_img = bridge.imgmsg_to_cv2(msg, "bgr8")
    except CvBridgeError as e:
        print(e)
    else:
        # Save your OpenCV2 image as a jpeg 
        fullPath = rootpath + str(count) + ".jpg"
        print("will be written...%s"%(fullPath))
        cv2.imwrite(fullPath, cv2_img)
        count-=1
        if count == 0:
            msg = String()
            msg.data = "start"
            pub.publish(msg)
            rospy.loginfo("Starting OCR...")



def main():
    rospy.init_node('image_listener')
    # Define your image topic
    image_topic = "/camera/color/image_raw"
    rospy.loginfo("start listening...")

    # Set up your subscriber and define its callback
    rospy.Subscriber(image_topic, Image, image_callback)
    # Spin until ctrl + c
    rospy.spin()

if __name__ == '__main__':
    sleep(1)
    print("5")
    sleep(1)
    print("4")
    sleep(1)
    print("3")
    sleep(1)
    print("2")
    sleep(1)
    print("1")
    main()